'''
常用串口指令
波特率9600 8 n 1 注意选择端口号
00 01 00 00 00 00  停止 
00 02 00 00 00 00  正转 
00 03 00 00 00 00  反转 
00 05 00 00 00 00  减速 
00 06 00 00 00 00  加速
00 52 00 xx 00 00  串口地址码修改
00 57 xx xx 00 00  速度细控  //2020年04月03号改为00 57 00 nn xx xx  ；nn是速度等级 xxxx是速度值
00 45 00 xx 00 00  速度等级 
00 4a 00 xx 00 00  转轴锁定xx=01锁定 00不锁定02-4 锁定一段时间释放
00 58 xx xx xx xx  直接输入转轴位置
00 59 xx xx xx xx  转轴位置累加
00 5a xx xx xx xx  转轴位置累减
00 5b xx xx xx xx  强制正转步数
00 5c xx xx xx xx  强制反转步数
00 34 63 00 00 00  恢复出厂值代码（从新通电起效）；
注：xx是输入的参数
'''


import serial

# 一些常用的指令初始化
zhiling = ['00 01 00 00 00 00', # 停止
'00 02 00 00 00 00', # 正转
'00 03 00 00 00 00', # 反转
'00 05 00 00 00 00', # 减速
'00 06 00 00 00 00', # 加速
'00 59 00 00 01 00', # 正转一定步数
'00 5C 00 00 19 00', # 反转一定步数
'00 67 00 00 00 00', # 查询位置
'A3 00 A2 A4 A5', # 查询体重秤信息
]

# 人体各部位比例，分别对应 肩部 胸部 腰部 臀部
humanProp = [0.84, 0.78, 0.6, 0.49] 
# 滑块的起始高度
baseheight = 56


# 前两个为电机控制器，第三个为体重秤
def init_motor_controller(com1, com2, com3):
    # 毫米波传感器端口连接
    ser1 = serial.Serial(com1, 9600, timeout=0.5)
    ser2 = serial.Serial(com2, 9600, timeout=0.5)
    ser3 = serial.Serial(com3, 9600, timeout=0.5)
    print('finish initing serial')
    return ser1, ser2, ser3


def distance2step(distance):
    # 步进点击旋转一圈垂直移动3.2cm左右
    return distance / 3.2 * 6400

def convert_step2hex(step):
    zhiling_bit = step
    zhiling_arr = [0, 0, 0, 0, 0, 0]
    index = 5
    while zhiling_bit > 0:
        temp_bit = zhiling_bit % 256
        zhiling_arr[index] = temp_bit
        index = index - 1
        zhiling_bit = zhiling_bit // 256
    # 这里加格式化
    return zhiling_arr

print(bytes.fromhex(zhiling[6]))
    












